Copyright □ 1999, by Professor Jong-Hwan Kim, Korea Advanced Institute of Science and Technology (KAIST).  All rights reserved. Reproduction or translation of any part of this work is unlawful. Request for permission or further information should be addressed to Professor Jong-Hwan Kim at <johkim@vivaldi.kaist.ac.kr> or <Tel: +82-42-869-3448>.

Prof. Jong-Hwan Kim	Intelligent Control Lab, KAIST

7.로봇 제어 프로그램 Ⅰ

이번 주부터 다룰 내용은 호스트 컴퓨터에서 사용되는 프로그램이다. Velocity, InitCommandData, Send_Command, Angle 함수에 대해서 설명을 한다.

7.1 Velocity 함수

모터를 구동하기 위해서 PWM(Pulse width modulation)방법을 사용한다. 0[eqn: sim]255사이의 값을 모터에 입력하는 값으로 사용한다. 모터를 양방향으로 제어해야 하므로 0[eqn: sim]255사이의 값인 PWM 신호의 중간값, 127을 0으로 보고 127보다 크면 양의 방향, 127보다 작으면 반대방향이라 설정한다. 즉, 127 일 때 모터는 정지하고 255일 때 시계방향으로 최고의 속도를 낸다면 0일 때 반시계 방향으로 최고의 속도를 낸다. 하지만 실제 모터 특성상 사구간(deadzone)과 포화영역(satruation)이 있어 이들을 고려해 주어야 모터제어를 잘할 수 있다. 그림 7.1과 같이 모터에 사구간이라는 영역이 존재하여 PWM 신호가 127근처에서 모터는 회전하지 않는다.

그림 7. 모터의 Deadzone 및 Saturation

어느 이상의 값을 주어야 모터가 회전하기 시작한다. 이를 위해 Velocity 함수가 필요하다. 또한, 모터의 선형특성을 주는 영역만을 고려하기 위하여 PWM 신호의 값이 어느 정도 이상이 되면 그 크기를 제한해준다. 이를 위해 Velocity 함수가 필요하다. 다음은 Velocity 함수의 코드이다. 여기서 열 개의 배열 command[i],i=0,[eqn: CDOTS],9,은 PWM 데이터를 저장하는 변수이다. 이 변수는 호스트 컴퓨터에서 로봇들에게 데이터를 보내줄 때 쓰인다.

void Velocity(int whichrobot, int vl, int vr)

{

if( vl < -106 )  vl = -106;	// 왼쪽 바퀴가 역방향일 때 PWM값의 최고값을 제한

if( vr < -106 )  vr = -106;	// 오른쪽 바퀴가 역방향일 때  PWM 값의 최고값을 제한

if( vl >= 106 )  vl = 106;	// 왼쪽 바퀴가 순방향일 때  PWM값의 최고값을 제한

if( vr >= 106 )  vr = 106;	// 오른쪽 바퀴가 순방향일 때  PWM 값의 최고값을 제한

if( vl >=0 )  vL = 136 + vl;	// 왼쪽 바퀴가 순방향일 때 최저값을 제한

else	vL = 118 + vl;	// 왼쪽 바퀴가 역뱡향일 때 최저값을 제한

if( vr >=0 ) vR = 136 + vr;	// 오른쪽 바퀴의 경우

else	vR = 118 + vr;

switch(whichrobot){	// 제어할 로봇을 선택

case HOME1 :	// HOME1 로봇에 대해서

command[4] = (BYTE)vL;	// 왼쪽 바퀴의 PWM 데이터를 command 배열에 저장

command[5] = (BYTE)vR;	// 오른쪽 바퀴의 PWM 데이터를 command 배열에 저장

break;

case HOME2:	// HOME2 로봇에 대해

command[6] = (BYTE)vL;

command[7] = (BYTE)vR;

break;

case HGOALIE:	// HOME3 = HGOALIE 로봇에 대해

command[8] = (BYTE)vL;

command[9] = (BYTE)vR;

break;

}

}

위의 코드에서는 모터의 선형구간을 고려하여 제어를 위하여 쓸 수 있는 PWM 신호 최대 값인 [eqn: +- 127]보다 작은 [eqn: +-]106까지를 사용하였고, 모터의 사구간을 피하기 위하여 모터가 순방향으로 회전하기 위해서는 136보다 크게 역방향으로 회전하는 경우는 118보다 작게 각각 설정하였다. 여기서 [eqn: v_l],과 [eqn: v_r]은 각각 Velocity 함수에서 왼쪽 바퀴 속도와 오른쪽 바퀴 속도를 입력받기 위한 변수이고 [eqn: V_L , V_R]은 [eqn: v_l , v_r]에서 모터의 사구간 영역과 포화영역을 고려하여 실제 모터에 인가되는 PWM 값을 저장하는 변수이다.

7.2 InitCommandData 함수

InitCommandData 함수는 로봇들에게 초기 데이터를 보내는 함수로, 이 프로그램이 시작되면 로봇들을 정지하게 만든다. 통신에 쓰이는 데이터 포맷은 다음과 같다. 즉, 처음에 3 바이트의 0xff는 통신을 위한 초기 값들이고, 4번째의 0x0f는 팀을 나타내는 데이터이다. 여기서는 A팀의 경우 0x0f를, B팀의 경우는 0xf0를 사용한다. 5번째에서 10번째까지 6 바이트는 로봇세대의 왼쪽과 오른쪽 바퀴의 PWM 값들이다. 여기서 이 값들이 0x7f로 설정되어 있으므로 로봇들은 정지해 있다.

void InitCommandData()

{

command[0] = 0xff;	// 통신을 위한 초기값

command[1] = 0xff;	// 통신을 위한 초기값

command[2] = 0xff;	// 통신을 위한 초기값

command[3] = 0x0f;	// 팀을 나타내는 데이터, 여기서는 A팀을 나타낸다.

command[4] = 0x7f;	// HOME1의 왼쪽 바퀴의 PWM 데이터

command[5] = 0x7f;	// HOME1의 오른쪽 바퀴의 PWM 데이터

command[6] = 0x7f;	// HOME2의 왼쪽 바퀴의 PWM 데이터

command[7] = 0x7f;	// HOME2의 오른쪽 바퀴의 PWM 데이터

command[8] = 0x7f;	// HGOALIE의 왼쪽 바퀴의 PWM 데이터

command[9] = 0x7f;	// HGOALIE의 오른쪽 바퀴의 PWM 데이터

}

7.3 Send_Command 함수

Send_command 함수는 Velocity 함수나 InitCommand 함수에서 command 배열에 저장한 값을 직렬포트를 통해 실제 로봇에 보내주는 함수이다.

void Send_Command()

{

m_pComm.WriteCommBlock((LPSTR)command, 10);	// 10 바이트의 command 배열의

데이터를 보내준다.

}

Send_command 함수를 사용하기 전에 미리 직렬포트에 대해서 설정을 해야 하는데 이에 대한 설명은 생략하겠다.

7.4 Angle 함수

Angle 함수는 로봇을 원하는 각도로 향하게 하는 함수이다. 즉, 비전시스템에서 로봇의 좌표를 받아서 원하는 각도로 향하게 한다.

그림 7. 로봇의 각도 제어

그림 7.2에서 살펴보면, 로봇과 원하는 지점사이의 x와 y의 거리차가 dx, dy라고 할 때 이 두 값과 atan2 함수를 이용하면 원하는 각도([eqn: theta_d])를 계산할 수 있다. 이때 원하는 각도와 로봇이 향한 각도([eqn: theta_h])와의 차가 각도 오차([eqn: theta_e])가 된다. 그림 7.2의 경우에서는 로봇의 왼쪽 바퀴를 순방향으로, 오른쪽 바퀴를 역방향으로 회전하면 된다. 이때 왼쪽바퀴 속도와 오른쪽 바퀴의 속도는 다음과 같다:

[eqn: &V_L = -&K_p CDOT theta_e# &V_R = &K_p CDOT theta_e]

(7.1)

이러한 원리를 프로그램화 한 것이 Angle 함수로 이 함수의 코드는 다음과 같다. 여기서는 원하는 각도를 함수의 인자로 주고, 로봇의 각도는 AngleOfHomeRobot[whichrobot]에서 얻을 수 있다. 이 값의 범위는 [eqn: 0DEG sim 360 DEG]이다.

void Angle(int whichrobot, int desired_angle)

{

int theta_e, vl, vr;	// [eqn: theta_e], [eqn: V_l , V_r] 변수 선언

theta_e = desired_angle - AngleOfHomeRobot[whichrobot];	// [eqn: theta_e = theta_d - theta_h]

while(theta_e >180)  theta_e -= 360;	//[eqn: theta_e]의 범위를 [eqn: -180 DEG sim 180 DEG]까지 제한

while(theta_e <-180) theta_e += 360;

if(theta_e<-90)	theta_e += 180;	// 로봇이 양방향으로 변환하는 것을 고려

else if(theta_e>90)	theta_e -= 180;	//[eqn: theta_e]가 90[eqn: DEG]보다 크면 -180[eqn: DEG], -90[eqn: DEG]

보다 작으면 +180[eqn: DEG]를 해준다.

if(abs(theta_e) > 50){	// [eqn: left |theta_e right |]가 [eqn: 50 DEG]보다 클 때

vl = (int)(-9./90.0*(double)theta_e);	// 왼쪽 바퀴의 속도 데이터

vr = (int)(9./90.0*(double)theta_e);	// 오른쪽 바퀴의 속도 데이터

}

else if(abs(theta_e)>20){	//  [eqn: 20 DEG][eqn: <left |theta_e right |<=] [eqn: 50DEG]일 때

vl = (int)(-10./90.0*(double)theta_e);

vr = (int)(10./90.0*(double)theta_e);

}

else{	//  0[eqn: DEG][eqn: <left |theta_e right |<=] 20[eqn: DEG]일 때

vl = (int)(-11.0/90.0*(double)theta_e);

vr = (int)(11.0/90.0*(double)theta_e);

}

Velocity(whichrobot, vl, vr);	// Velocity 함수 호출

}

위의 코드에서 알수 있듯이 각도 오차를 50[eqn: DEG]보다 클 때, 20[eqn: DEG]와 50[eqn: DEG]사이일 때, 그리고 0[eqn: DEG]에서 20[eqn: DEG]사이일때와 같이 세단계로 나누어 제어를 하였다. 이와 같이 한 이유는 각도 오차가 클 때 사용한 제어 계수를 각도가 작을 때 그대로 사용하면 제어입력 값이 작아지므로 이를 보상하기 위함이다.